home *** CD-ROM | disk | FTP | other *** search
/ IRIS Performer 2.2 Friends Demo / SGI IRIS Performer 2.2 Friends Demo.iso / friends / medit / pfLoader / Internalstuff / engines.c < prev    next >
Text File  |  1997-11-20  |  12KB  |  497 lines
  1. /************************************************************************
  2. Extract information from a bezier (see your text book!)
  3. ************************************************************************/
  4. #define BezierPosition(axis)     ((b0*p0[axis]) + (b1*p1[axis]) + (b2*p2[axis]) + (b3*p3[axis]))
  5. #define BezierVelocity(axis)     ((b0*(p1[axis]-p0[axis])) + (b1*(p2[axis]-p1[axis])) + (b2*(p3[axis]-p2[axis])))
  6. #define BezierAcceleration(axis) ((b0*(p0[axis]+p2[axis]-(2*p1[axis]))) + (b1*(p1[axis]+p3[axis]-(2*p2[axis]))))
  7. #define BezierAccelAccel(axis)     ( b0*((p3[axis]-p0[axis]) + (b1*(p1[axis]-p2[axis]))))
  8.  
  9. void BezierCalc(reg BezierDataPtr b, reg real t, reg int request, reg real (*result)[XYZ])
  10. {
  11.     reg real b0, b1, b2, b3;
  12.     reg real *p0, *p1, *p2, *p3;
  13.     reg real t1, t2, t3, t12, t13;
  14.  
  15.     t2 = t*t;        /* Powers of t and (1-t) */
  16.     t3 = t2*t;
  17.     t1 = 1.0-t;
  18.     t12 = t1*t1;
  19.     t13 = t12*t1;
  20.  
  21.     p0 = b->ControlPoint[0];
  22.     p1 = b->ControlPoint[1];
  23.     p2 = b->ControlPoint[2];
  24.     p3 = b->ControlPoint[3];
  25.  
  26.     if (request & BPosition) {
  27.     b0 = t13;
  28.     b1 = 3.0*t*t12;
  29.     b2 = 3.0*t2*t1;
  30.     b3 = t3;
  31.     result[0][X] = BezierPosition(X);
  32.     result[0][Y] = BezierPosition(Y);
  33.     result[0][Z] = BezierPosition(Z);
  34.     result++;
  35.     }
  36.  
  37.     if (request & BVelocity) {
  38.     b0 = 3.0*t12;
  39.     b1 = 6.0*t*t1;
  40.     b2 = 3.0*t2;
  41.     result[0][X] = BezierVelocity(X);
  42.     result[0][Y] = BezierVelocity(Y);
  43.     result[0][Z] = BezierVelocity(Z);
  44.     result++;
  45.     }
  46.  
  47.     if (request & BAcceleration) {
  48.     b0 = -6.0*t1;
  49.     b1 =  6.0*t;
  50.     result[0][X] = BezierAcceleration(X);
  51.     result[0][Y] = BezierAcceleration(Y);
  52.     result[0][Z] = BezierAcceleration(Z);
  53.     result++;
  54.     }
  55.  
  56.     if (request & BAccelAccel) {
  57.     b0 = 6.0;
  58.     b1 = 3.0;
  59.     result[0][X] = BezierAccelAccel(X);
  60.     result[0][Y] = BezierAccelAccel(Y);
  61.     result[0][Z] = BezierAccelAccel(Z);
  62.     }
  63. }
  64. /************************************************************************
  65. Convert distance along a path to bezier/t
  66. ************************************************************************/
  67. static boolean ChopMotionData(reg PathPtr p, reg real distance, reg BezierDataPtr *bezier, reg real *t)
  68. {
  69.     reg BezierDataPtr s;
  70.     reg int i, j, m, last, count;
  71.     reg real start, end, length, segsize;
  72.  
  73.     if (p) {
  74. #if ITSTHEMODELLER
  75.     if (!p->Valid OR !p->NoCurves OR !p->Curve) {
  76.         GenerateBezierData(p);
  77.     }
  78. #endif
  79.     if (!p->NoCurves OR !p->Curve) {
  80.         return FALSE;
  81.     }
  82.     if (distance < 0) {
  83.         *bezier = p->Curve;
  84.         *t = 0;
  85.         return TRUE;
  86.     }
  87.     else if (distance > p->CurveLength) {
  88.         *bezier = p->Curve+(p->NoCurves-1);
  89.         *t = 1.0;
  90.         return TRUE;
  91.     }
  92.  
  93.     i = 0;                /* Binary chop the list of bezier subcurves */
  94.     j = p->NoCurves;
  95.     count = 0;
  96.     until (TheCowsAreHome) {
  97.         count++;
  98.         m = (i+j)>>1;
  99.         s = p->Curve+m;
  100.         start = s->Start;
  101.         length = s->Length;
  102.         end = start + length;
  103.         if (distance < start) {
  104.         j = m;
  105.         }
  106.         else if (distance > end) {
  107.         if (i IS m) {        /* Avoid infinite loop when ((j-i) IS 1) */
  108.             i = j;
  109.         }
  110.         else {
  111.             i = m;
  112.         }
  113.         }
  114.         else {
  115.         break;
  116.         }
  117.     }
  118.     *bezier = s;
  119.     
  120.     if (!s->l) {
  121.         return FALSE;
  122.     }
  123.  
  124.     i = 0;                /* Then binary chop the parameterisation */
  125.     j = s->Subdivision;
  126.     last = j-1;
  127.     distance -= start;
  128.     until (TheCowsAreHome) {
  129.         count++;
  130.         m = (i+j)>>1;
  131.         start = s->l[m];
  132.         if (m IS last) {
  133.         length = s->Length - start;
  134.         }
  135.         else {
  136.         length = s->l[m+1] - start;
  137.         }
  138.         end = start + length;
  139.         if (distance < start) {
  140.         j = m;
  141.         }
  142.         else if (distance > end) {
  143.         if (i IS m) {
  144.             i = m = j;
  145.         }
  146.         else {
  147.             i = m;
  148.         }
  149.         }
  150.         else {
  151.         break;
  152.         }
  153.     }
  154.     segsize = 1.0/s->Subdivision;
  155.     *t = ((real)m * segsize) + (((distance-start)/length) * segsize);
  156.     return TRUE;
  157.     }
  158.     return FALSE;
  159. }
  160. /************************************************************************
  161. The data passed around during the engine updates
  162. ************************************************************************/
  163. typedef struct EngineCalcStruct {
  164. real        Value;            /* Value to output            */
  165. real        StartTime;        /* Time reference            */
  166.  
  167. /* Parameters for bezier calcs */
  168. real        t;
  169. PathPtr        path;
  170. BezierDataPtr    bezier;
  171. flag        HasBezier;
  172. } EngineCalc;
  173. typedef EngineCalc *EngineCalcPtr;
  174.  
  175. static void InitEngineCalc(reg EngineCalcPtr e)
  176. {
  177.     e->Value = 0.0;
  178.     e->StartTime = 0.0;
  179.     e->HasBezier = FALSE;
  180. }
  181. /************************************************************************
  182. Initialise an engine at the start
  183. ************************************************************************/
  184. static void EngineInitialiser(rconst TreePtr t, rconst void *start)
  185. {
  186.     reg TreePtr c;
  187.  
  188.     t->Active = t->StartActive;
  189.     t->StartTime = *(real*)start;
  190.     switch (t->Type) {
  191.     case AngleEngine:   t->Angle = t->Offset;
  192.                 break;
  193.     case DcsOutput:        c = t->Connection;
  194.                 if (c) {
  195.                 c->OutputX = c->OutputBX =
  196.                 c->OutputY = c->OutputBY =
  197.                 c->OutputZ = c->OutputBZ =
  198.                 c->OutputH = c->OutputBH =
  199.                 c->OutputP = c->OutputBP =
  200.                 c->OutputR = c->OutputBR = FALSE;
  201.                 }
  202.                 break;
  203.     }
  204. }
  205. /************************************************************************
  206. Update engines
  207. ************************************************************************/
  208. #define CopyEngineCalc(dest, src) memcpy(dest, src, sizeof(EngineCalc))
  209. #define ResetE if (!FirstOne) { CopyEngineCalc(e, &start); FirstOne = FALSE; }
  210.  
  211. #define DoOutput(output, what)    \
  212. if (t->output) {        \
  213.     c->output = TRUE;        \
  214.     c->DcsValue.what = e->Value;\
  215. }
  216. static void EngineUpdater(reg TreePtr t, reg EngineCalcPtr e)
  217. {
  218.     EngineCalc start;
  219.  
  220.     reg PathPtr p;
  221.     reg TreePtr c, recurse;
  222.     reg real angle, distance;
  223.     reg flag FirstOne = TRUE, DontRecurse;
  224.  
  225.     CopyEngineCalc(&start, e);
  226.     while (t) {
  227.     if (t->Active) {
  228.         DontRecurse = FALSE;
  229.         switch (t->Type) {
  230.         case TimeEngine:    ResetE;
  231.                     e->Value = ElapsedTime - e->StartTime;
  232.                     break;
  233.     
  234.         case LineEngine:    ResetE;
  235.                     e->Value = ((ElapsedTime-t->StartTime)*t->Scale) + t->Offset;
  236.                     break;
  237.     
  238.         case AngleEngine:    ResetE;
  239.                     if (t->StartTime ISNT ElapsedTime) {
  240.                         t->Angle += FrameTime*t->Scale;
  241.                         while (t->Angle > 360.0) {
  242.                         t->Angle -= 360.0;
  243.                         }
  244.                         while (t->Angle < -360.0) {
  245.                         t->Angle += 360.0;
  246.                         }
  247.                         t->StartTime = ElapsedTime;
  248.                     }
  249.                     e->Value = t->Angle;
  250.                     break;
  251.     
  252.         case WaveEngine:    ResetE;
  253.                     angle = (t->NoWaves * (ElapsedTime/t->WaveTime)) + t->Offset;
  254.                     e->Value = sin((angle*(2.0*M_PI))+rad(t->Phase)) * t->Scale;
  255.                     break;
  256.     
  257.         case SensorEngine:    ResetE;
  258.                     break;
  259.     
  260.         case ScalarEngine:    e->Value = (e->Value * t->Scale) + t->Offset;
  261.                     ResetE;
  262.                     break;
  263.     
  264.         case SplineEngine:    ResetE;
  265.                     p = t->PathData;
  266.                     distance = e->Value;
  267.                     if (p->Closed) {
  268.                         distance = fmod(distance, p->CurveLength);
  269.                     }
  270.                     if (ChopMotionData(p, distance, &(e->bezier), &(e->t))) {
  271.                         e->HasBezier = TRUE;
  272.                         e->path = t->PathData;
  273.                     }
  274.                     break;
  275.     
  276.         case CompareEngine:    ResetE;
  277.                     if (t->CompareType IS 0) {
  278.                         DontRecurse = (e->Value <= t->Scale);
  279.                     }
  280.                     else {
  281.                         DontRecurse = (e->Value >= t->Scale);
  282.                     }
  283.                     break;
  284.  
  285.         case DcsOutput:        c = t->Connection;
  286.                     if (c) {
  287.                         ResetE;
  288.                         if (e->HasBezier) {
  289.                         c->t = e->t;
  290.                         c->path = e->path;
  291.                         c->bezier = e->bezier;
  292.                         c->OutputBX = t->OutputX;
  293.                         c->OutputBY = t->OutputY;
  294.                         c->OutputBZ = t->OutputZ;
  295.                         c->OutputBH = t->OutputH;
  296.                         c->OutputBP = t->OutputP;
  297.                         c->OutputBR = t->OutputR;
  298.                         }
  299.                         else {
  300.                         DoOutput(OutputX, x);
  301.                         DoOutput(OutputY, y);
  302.                         DoOutput(OutputZ, z);
  303.                         DoOutput(OutputH, h);
  304.                         DoOutput(OutputP, p);
  305.                         DoOutput(OutputR, r);
  306.                         }
  307.                     }
  308.                     break;
  309.  
  310.         case SwitchOutput:    c = t->Connection;
  311.                     if (c) {
  312.                         c->Visible = t->SwitchOn;
  313.                     }
  314.                     break;
  315.         
  316.         case ActivatorEngine:    c = t->Connection;
  317.                     if (c) {
  318.                         c->Active = TRUE;
  319.                         c->StartTime = ElapsedTime;
  320.                     }
  321.                     break;
  322.     
  323.         case DeactivatorEngine:    c = t->Connection;
  324.                     if (c) {
  325.                         c->Active = FALSE;
  326.                     }
  327.                     break;
  328.  
  329.         }
  330.         if (!DontRecurse) {
  331.         recurse = t->Across;
  332.         if (recurse) {
  333.             EngineUpdater(recurse, e);
  334.         }
  335.         }
  336.     }
  337.     t = t->Down;
  338.     }
  339. }
  340. /************************************************************************
  341. Generate a DCS matrix
  342. ************************************************************************/
  343. #if ITSTHEMODELLER
  344.  
  345. /* Bridge the from Performer to ModelWorks internal routines */
  346. #define pfArcTan2(y, x) deg(atan2f(y, x))
  347. #define SinCos(a, s, c) { s = sinf(rad(a)); c = cosf(rad(a)); }
  348. #else
  349.  
  350. /* ditto ModelWorks to Performer */
  351. #define SmallMatrix pfMatrix
  352. #define SmallMatrixPtr pfMatrix
  353. #define MultSmallMatrix(a, b, dst) pfMultMat(dst, a, b)
  354. #define IdentifySmall(m) pfMakeIdentMat(m)
  355. #define CopySmallMatrix(dst, src) pfCopyMat(dst, src)
  356.  
  357. #define PreTranslateSmall(m, x, y, z) pfPreTransMat(m, x, y, z, m);
  358. #define PreYawSmall(m, a)    pfPreRotMat(m, a, 0, 0, 1, m);
  359. #define PrePitchSmall(m, a)  pfPreRotMat(m, a, 1, 0, 0, m);
  360. #define PreRollSmall(m, a)   pfPreRotMat(m, a, 0, 1, 0, m);
  361.  
  362. #define SinCos(a, s, c) pfSinCos(a, &s, &c);
  363. static
  364. #endif
  365. void CalcPitchAndHeading(float v[XYZ], float *pitch, float *heading)
  366. {
  367.     float sinh, cosh;
  368.     reg float h, x, y;
  369.  
  370.     x = v[X]; y = v[Y];
  371.     if ((x IS 0.0) AND (y IS 0.0)) {
  372.     *pitch = 90;
  373.     *heading = 0;
  374.     }
  375.     else {
  376.     h = *heading = pfArcTan2(y, x);
  377.     SinCos(-h, sinh, cosh);
  378.     x = (x*cosh) - (y*sinh);
  379.     *pitch = -pfArcTan2(v[Z], x);
  380.     }
  381. }
  382. boolean MakeDcsMatrix(reg TreePtr Dcs, reg SmallMatrixPtr m)
  383. {
  384.     reg real *c;
  385.     real bv[4][3];
  386.     reg PathPtr path;
  387.     reg float x, y, z;
  388.     float h, p, ph[3];
  389.     reg BezierDataPtr bezier;
  390.     reg flag DoXYZ, DoHPR, DoBXYZ, DoBHPR, hold;
  391.     reg flag ox, oy, oz, oh, op, or, obx, oby, obz, obh, obp, obr;
  392.  
  393.     ox = Dcs->OutputX;    oy = Dcs->OutputY;  oz = Dcs->OutputZ;
  394.     oh = Dcs->OutputH;    op = Dcs->OutputP;  or = Dcs->OutputR;
  395.     DoXYZ = ox OR oy OR oz;
  396.     DoHPR = oh OR op OR or;
  397.  
  398.     obx = Dcs->OutputBX;  oby = Dcs->OutputBY;  obz = Dcs->OutputBZ;
  399.     obh = Dcs->OutputBH;  obp = Dcs->OutputBP;  obr = Dcs->OutputBR;
  400.     DoBXYZ = obx OR oby OR obz;
  401.     DoBHPR = obh OR obp OR obr;
  402.  
  403.     hold = Dcs->HoldValues;
  404.     if (DoXYZ OR DoHPR OR DoBXYZ OR DoBHPR) {
  405.     if (DoBXYZ OR DoBHPR) {
  406. #if ITSTHEMODELLER
  407.         IdentifySmall(m);
  408. #else
  409.         CopySmallMatrix(m, Dcs->DcsInverse);
  410. #endif
  411.         path = Dcs->path;
  412.         bezier = Dcs->bezier;
  413.         c = path->Curve[0].ControlPoint[0];
  414.         BezierCalc(bezier, Dcs->t, BPosition|BVelocity, bv);
  415.         if (DoBXYZ) {
  416.         x = y = z = 0.0;
  417.         if (obx) {
  418.             if (!hold) Dcs->OutputBX = FALSE;
  419.             x = bv[0][X]-c[X];
  420.         }
  421.         if (oby) {
  422.             if (!hold) Dcs->OutputBY = FALSE;
  423.             y = bv[0][Y]-c[Y];
  424.         }
  425.         if (obz) {
  426.             if (!hold) Dcs->OutputBZ = FALSE;
  427.             z = bv[0][Z]-c[Z];
  428.         }
  429.         PreTranslateSmall(m, x, y, z);
  430.         }
  431.         if (DoBHPR) {
  432.         c = path->Curve[0].ControlPoint[0];
  433.         PreTranslateSmall(m, c[X], c[Y], c[Z]);
  434.         CopyXYZ(ph, bv[1]);
  435.         CalcPitchAndHeading(ph, &p, &h);
  436.         if (obh) {
  437.             if (!hold) Dcs->OutputBH = FALSE;
  438.             PreYawSmall(m, h-path->HeadingOffset);
  439.         }
  440.         if (obp) {
  441.             if (!hold) Dcs->OutputBP = FALSE;
  442.             PrePitchSmall(m, p-path->PitchOffset);
  443.         }
  444.         PreTranslateSmall(m, -c[X], -c[Y], -c[Z]);
  445.         }
  446.         MultSmallMatrix(Dcs->DcsSmall, m, m);
  447.     }
  448.     else {
  449. #if ITSTHEMODELLER
  450.         CopySmallMatrix(m, Dcs->DcsSmall);
  451. #else
  452.         pfMakeIdentMat(m);
  453. #endif
  454.         
  455.     }
  456.  
  457.     if (DoXYZ) {
  458.         x = y = z = 0.0;
  459.         if (ox) {
  460.         if (!hold) Dcs->OutputX = FALSE;
  461.         x = Dcs->DcsValue.x;
  462.         }
  463.         if (oy) {
  464.         if (!hold) Dcs->OutputY = FALSE;
  465.         y = Dcs->DcsValue.y;
  466.         }
  467.         if (oz) {
  468.         if (!hold) Dcs->OutputZ = FALSE;
  469.         z = Dcs->DcsValue.z;
  470.         }
  471.         PreTranslateSmall(m, x, y, z);
  472.     }
  473.  
  474.     if (DoHPR) {
  475.         if (oh) {
  476.         if (!hold) Dcs->OutputH = FALSE;
  477.         PreYawSmall(m, Dcs->DcsValue.h);
  478.         }
  479.         if (op) {
  480.         if (!hold) Dcs->OutputP = FALSE;
  481.         PrePitchSmall(m, Dcs->DcsValue.p);
  482.         }
  483.         if (or) {
  484.         if (!hold) Dcs->OutputR = FALSE;
  485.         PreRollSmall(m, Dcs->DcsValue.r);
  486.         }
  487.     }
  488.  
  489. #if ITSTHEMODELLER
  490.         MultSmallMatrix(Dcs->DcsInverse, m, m);
  491. #endif
  492.     return TRUE;
  493.     }
  494.     IdentifySmall(m);
  495.     return FALSE;
  496. }
  497.